/*
 * @Author: zwf 240970521@qq.com
 * @Date: 2023-07-18 19:15:42
 * @LastEditors: zwf 240970521@qq.com
 * @LastEditTime: 2023-08-20 09:35:03
 * @FilePath: /esp32_motor/motor/algorithm /openLoop.c
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#include "motor.h"
#include "esp_timer.h"
#include "math_common.h"
#include "motor_common.h"
#include "motor_priv.h"
#include "smbObserver.h"
#include "fluxObserver.h"
#include "IQmathLib.h"


static _iq24  openControlAngle = _IQ(0);
static _iq24  noAngle          = _IQ(0);
static SinCos sincos;
static long   time1, time2, time3, time4, time5, time6, time7, time8, time9, time10, time11;
/*开环状态更新*/
static void MOTOR_dataFrameUpdate(MotorDev *dev)
{
    MotorObj      *priv = NULL;
    DataSendFrame *frame;

    if (!dev || !dev->priv)
    {
        return;
    }

    priv  = dev->priv;
    frame = &priv->dataFrame;
    /*赋值*/
    frame->fData[0] = _IQtoF(priv->curAngle.angle);
    frame->fData[1] = _IQtoF(openControlAngle);
    frame->fData[2] = _IQtoF(fluxOb.err);
    // frame->fData[4] = _IQ27toF(fluxOb.lambdaEst);
    // frame->fData[5] = _IQ27toF(fluxOb.vAlphaPrev);
    // frame->fData[6] = _IQ27toF(fluxOb.vBetaPrev);
    // frame->fData[7] = _IQ27toF(fluxOb.ctrlTs);
    // frame->fData[8] = _IQ27toF(fluxOb.iAlphaPrev);
    // frame->fData[9] = _IQ27toF(fluxOb.iBetaPrev);
    // frame->fData[0] = (double)(time2 - time1);
    // frame->fData[1] = (double)(time3 - time2);
    // frame->fData[2] = (double)(time4 - time3);
    // frame->fData[3] = (double)(time5 - time4);
    // frame->fData[4] = (double)(time6 - time5);
    // frame->fData[5] = (double)(time7 - time6);
    // frame->fData[6] = (double)(time8 - time7);
    // frame->fData[7] = (double)(time9 - time8);
    // frame->fData[8] = (double)(time10 - time9);
    // frame->fData[9] = _IQtoF(sincos.cosValue);
    // frame->fData[10] = _IQtoF(sincos.sinValue);
    frame->dataNum = 3;
    /*发送当前的状态到上位机*/
    xQueueSendFromISR(dataQueue, &priv->dataFrame, 0);
}

/*开环控制*/
void MOTOR_speedOpenCycle(MotorDev *dev)
{
    MotorObj *priv = NULL;

    if (!dev || !dev->priv)
    {
        return;
    }

    priv             = dev->priv;

    time1            = esp_timer_get_time();
    openControlAngle += priv->setSpeed;
    // 通过乘以时间间隔和目标速度来计算需要转动的机械角度，对轴角度进行归一化，以确保其值在 0 到 2π 之间。
    openControlAngle = ElectricalIQAngle(openControlAngle);
    if (priv->setAngle !=0)
    {
        FastSinCos(priv->setAngle, &sincos);
    }
    else
    {
        FastSinCos(openControlAngle, &sincos);
    }
    
    MOTOR_updateAmpere(dev);
    ClarkTrans(&priv->ampare);
    ParkTrans(&priv->ampare, sincos);
    priv->ampare.q = priv->iqFilter->lowPassFilter(priv->iqFilter, priv->ampare.q);
    priv->ampare.d = priv->idFilter->lowPassFilter(priv->idFilter, priv->ampare.d);
    MOTOR_locationUpdate(dev);
    ParkReversalTrans(&priv->voltage, sincos);
    ClarkReversalTrans(&priv->voltage);
    priv->setPhaseVoltage(dev);
    time2 = esp_timer_get_time();
    MOTOR_dataFrameUpdate(dev);
    /*计算整个流程耗时*/
}